/**
  ****************************(C) COPYRIGHT 2022 jk****************************
  * @file       gimbal_task.c/h
  * @brief      完成云台控制任务：
				yaw采用了舵机控制，主要实现三个位置方向：前视角，后视角，辅助视角   A板上A接口
				pitch采用了舵机控制   A板上B接口
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. add some annotation
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2022 jk****************************
  */

#include "gimbal_task.h"
#include "cmsis_os.h"
#include "ins_task.h"
#include "detect_task.h"
#include "CAN_receive.h"
#include "obtein_mineral_task.h"

extern TIM_HandleTypeDef htim5;
extern TIM_HandleTypeDef htim8;

  /**
	* @brief         init
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void ginbal_init(gimbal_move_t* gimbal_init);
/**
  * @brief          设置云台控制模式，主要在'gimbal_behaviour_mode_set'函数中改变
  * @param[out]     gimbal_set_mode:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_mode(gimbal_move_t* set_mode);
/**
  * @brief          设置云台控制状态，主要在'gimbal_behaviour_mode_set'函数中改变
  * @param[out]     gimbal_set_mode:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_state(gimbal_move_t* set_sate);
/**
  * @brief          设置云台控制设定值，控制值是通过gimbal_behaviour_control_set函数设置的
  * @param[out]     gimbal_set_control:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_control(gimbal_move_t* set_control);
/**
  * @brief          云台行为控制，根据不同行为采用不同控制函数
  * @param[out]     add_yaw:设置的yaw角度增加值，单位 rad
  * @param[out]     add_pitch:设置的pitch角度增加值，单位 rad 
  * @param[in]      gimbal_mode_set:云台数据指针
  * @retval         none
  */
static void gimbal_control_set(uint16_t* add_yaw, fp32* add_pitch, gimbal_move_t* gimbal_control);

gimbal_move_t gimbal_move;
engineering_obtein_move_t  *gimbal_survive;

 fp32 angle_mouse_z=0,add_pitch=0;
int a;
  /**
	* @brief          夹取任务任务，间隔 CHASSIS_CONTROL_TIME_MS 2ms
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
void gimbal_task(void const* pvParameters)
{
	//空闲一段时间
	vTaskDelay(gimbal_task_INIT_TIME);
	ginbal_init(&gimbal_move);
	while (toe_is_error(chassis_A_TOE))
	{
		vTaskDelay(100);
	}
	while (1)
	{
		gimbal_set_mode(&gimbal_move);
		gimbal_set_state(&gimbal_move);
		gimbal_set_control(&gimbal_move);
		//给出pwm的值
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, gimbal_move.yaw_set_pwm);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, gimbal_move.pitch_set_pwm);
		//vTaskDelay(5);
	}
}


/**
  * @brief         init
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void ginbal_init(gimbal_move_t* gimbal_int)
{
	if (gimbal_int == NULL)
	{
		return;
	}
	gimbal_int->gimbal_rc = get_remote_control_point();
	gimbal_int->can_rc_chassis = get_chassis_A_point();

	const static fp32 gimbal_yaw_filter[1] = { gimbal_yaw_NUM };
	const static fp32 gimbal_pitch_filter[1] = { gimbal_pitch_NUM };

	gimbal_int->gimbal_mode = gimbal_ZERO_FORCE;
	gimbal_int->gimbal_state = yaw_front;
    gimbal_survive=&engineering_obtein_move;
    
	first_order_filter_init(&gimbal_int->gimbal_cmd_slow_set_yaw, engineering_CONTROL_TIME, gimbal_yaw_filter);
	first_order_filter_init(&gimbal_int->gimbal_cmd_slow_set_pitch, engineering_CONTROL_TIME, gimbal_pitch_filter);
}

/**
  * @brief          设置云台控制模式，主要在'gimbal_behaviour_mode_set'函数中改变
  * @param[out]     gimbal_set_mode:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_mode(gimbal_move_t* set_mode)
{
	if (set_mode == NULL)
	{
		return;
	}
	if (switch_is_down(set_mode->gimbal_rc->rc.s[0]))
	{
		set_mode->gimbal_mode = gimbal_ZERO_FORCE;
	}
	else
	{
		set_mode->gimbal_mode = gimbal_CONTROL;
	}
}

/**
  * @brief          设置云台控制状态，主要在'gimbal_behaviour_mode_set'函数中改变
  * @param[out]     gimbal_set_mode:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_state(gimbal_move_t* set_sate)
{
	if (set_sate == NULL)
	{
		return;
	}
	if (gimbal_survive->can_chassis_A->button_limit_flg)
	{
		set_sate->gimbal_state = yaw_front;  //前面
	}
	else 
	{
		set_sate->gimbal_state = yaw_back;
	}
}


/**
  * @brief          设置云台控制设定值，控制值是通过gimbal_behaviour_control_set函数设置的
  * @param[out]     gimbal_set_control:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_control(gimbal_move_t* set_control)
{
	if (set_control == NULL)
	{
		return;
	}

	uint16_t yaw_pwm = 0;
	fp32 pitch_pwm = 0.0f;

	//获取两个控制值
	gimbal_control_set(&yaw_pwm, &pitch_pwm , set_control);
	//PWM限制
	if (yaw_pwm > servo_max_pwm)
	{
		yaw_pwm = servo_max_pwm;
	}
	else if (yaw_pwm < servo_min_pwm)
	{
		yaw_pwm = servo_min_pwm;
	}
	if (pitch_pwm > servo_max_pwm)
	{
		pitch_pwm = servo_max_pwm;
	}
	else if (pitch_pwm < servo_min_pwm)
	{
		pitch_pwm = servo_min_pwm;
	}

	/*first_order_filter_cali(&set_control->gimbal_cmd_slow_set_yaw, yaw_pwm);
	first_order_filter_cali(&set_control->gimbal_cmd_slow_set_pitch, pitch_pwm);

	set_control->yaw_set_pwm = set_control->gimbal_cmd_slow_set_yaw.out;
	set_control->pitch_set_pwm = set_control->gimbal_cmd_slow_set_pitch.out;*/

	set_control->yaw_set_pwm = yaw_pwm;
	set_control->pitch_set_pwm = pitch_pwm;
}


/**
  * @brief          云台行为控制，根据不同行为采用不同控制函数
  * @param[out]     add_yaw:设置的yaw角度增加值，单位 rad
  * @param[out]     add_pitch:设置的pitch角度增加值，单位 rad
  * @param[in]      gimbal_mode_set:云台数据指针
  * @retval         none
  */

static void gimbal_control_set(uint16_t* add_yaw, fp32* add_pitch, gimbal_move_t* gimbal_control)
{
	if (add_yaw == NULL || add_pitch == NULL || gimbal_control == NULL)
	{
		return;
	}

	//添加按键控制YAW

	if (gimbal_control->gimbal_mode == gimbal_ZERO_FORCE)
	{
		*add_yaw = YAW_front;
		*add_pitch = 1000;
	}
	else if (gimbal_control->gimbal_mode == gimbal_CONTROL)
	{

		if (gimbal_control->gimbal_state == yaw_front)
		{
			*add_yaw = YAW_front;
		}
		else if (gimbal_control->gimbal_state == yaw_back)
		{
			*add_yaw = YAW_back;
			if (gimbal_survive->engineer_translation_motor_data.motor_measure->ecd_count > 2)
			{
				*add_yaw = YAW_back - gimbal_survive->engineer_translation_motor_data.motor_measure->ecd_count;
			}
			else if (gimbal_survive->engineer_translation_motor_data.motor_measure->ecd_count < -2)
			{
				*add_yaw = YAW_back - gimbal_survive->engineer_translation_motor_data.motor_measure->ecd_count;
			}
			else *add_yaw = YAW_back;
		}

		if (!(gimbal_move.gimbal_rc->key.v & KEY_PRESSED_OFFSET_SHIFT))
		{
			if ((gimbal_move.gimbal_rc->mouse.z) && (angle_mouse_z < 130)&&(angle_mouse_z>-130))
			{

				angle_mouse_z += 0.0005f*gimbal_move.gimbal_rc->mouse.z;

			}

		}

		*add_pitch = (1000 + fp32_constrain(angle_mouse_z, -130, 130));       //880——>1100
	  }
}


/******************************end of file************************************/

